% clear all;
% close all;
% clc;

m = 1;%小球质量
M = 5;%车质量
L = 2;%杆的质量
g = 10;%重力加速度
d = 1;%摩擦系数
I = m*L^2/30;%转动惯量 0.1333
C = (M+m)*(I+m*L^2)-m^2*L^2;%常数分母 20.8000

%linearized system
% 
% A = [ 0 1 0 0;
%     0 -(I+m*L^2)*d/C (-m^2*L^2*g*(M+m)*(I+m*L^2))/C^2 0;
%     0 0 0 1;
%     0 m*L*d/C (M+m)*m*g*L/C 0];

A = [ 0 1 0 0;
    0 -0.1987 -2.2929 0;
    0 0 0 1;
    0 0.0962 5.7692 0];

% A = [ 0 1 0 0;
%     0   -(I+m*L^2)*d/C   -m^2*L^2*g/C    0;
%     0 0 0 1;
%     0 m*L*d/C (M+m)*m*g*L/C 0];

% B = [0;(I+m*L^2)/C;0;-m*L/C];
B = [0;0.1987;0;-0.0962];

eig(A)%特征值
rank(ctrb(A,B))%是否可控
%-------------------------------------------
% p = [-4.2820,-1.9841,-3,-4];%特征根远离虚轴比较快
% %p = [-4.2820,-1.9841,-1,-2];%特征根靠近虚轴比较慢
% K = place(A,B,p);  %设定系统极点找到K
%-------------------------------------------
Q = [10 0 0 0; 
     0 1 0 0;
     0 0 10 0;
     0 0 0 1]
%R = 0.1
 R = 0.001
K = lqr(A,B,Q,R); %采用LQR找到K
%-------------------------------------------
eig(A-B*K)

tspan = 0:.05:10;
y0 = [0; 0; pi/6; 0.5];
%求解四个微分方程
[t,y] = ode45(@(t,y)pend_cart(y,I,m,M,L,g,d,-K*(y - [1;0;0;0])),tspan,y0);

figure;
grid on;
hold on;
plot(y(:,1));
plot(y(:,2));
plot(y(:,3));
plot(y(:,4));
legend({'x','$$ \dot{x}$$','$$\theta$$','$$\dot{\theta}$$'},'interpreter','latex');
hold off;
figure;
for k=1:length(t)
    draw_pend(y(k,:),m,M,L);
end
